// app
#include "balance.h"
#include "speed_estimation.h"
#include "linkNleg.h"
#include "fly_detection.h"
#include "robot_def.h"
#include "general_def.h"
#include "ins_task.h"
#include "dmmotor.h"
#include "LK9025.h"
#include "controller.h"
#include "can_comm.h"
#include "user_lib.h"
#include "remote_control.h"
#include "stdint.h"
#include "arm_math.h" // 需要用到较多三角函数
#include "bsp_dwt.h"
#include "buzzer.h"
static uint32_t balance_dwt_cnt;
static float del_t;

/* 底盘拥有的模块实例 */
static INS_t *imu_data;
static RC_ctrl_t *rc_data;                  // 调试用
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // syh
static CANCommInstance *ci;
// 四个关节电机和两个驱动轮电机
static DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2];
// 两个腿的参数,0为左腿,1为右腿
static LinkNPodParam l_side, r_side; // syh  phi5
static ChassisParam chassis;
// 综合运动补偿的PID控制器
static PIDInstance steer_p_pid, steer_v_pid;   // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
static PIDInstance anti_crash_pid, phi5_pid;   // 抗劈叉,将输出以相反的方向叠加到左右腿的上
static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
static PIDInstance legdot_pid_l, legdot_pid_r;
static PIDInstance roll_compensate_pid, rolldot_pid; // roll轴补偿,用于保持机体水平
static BuzzzerInstance *buzzer;
static Robot_Status_e chassis_status;
extern float leglenKp = 1500.0f, leglenKi = 0.0f, leglenKd = 120.0f;
void BalanceInit()
{
    rc_data = RemoteControlInit(&huart3);
    imu_data = INS_Init();
    Buzzer_config_s buzzer_config = {
        .alarm_level = ALARM_LEVEL_HIGH, // 设置警报等级 同一状态下 高等级的响应
        .loudness = 20,                  // 设置响度
        .octave = OCTAVE_1,              // 设置音阶
    };
    buzzer = BuzzerRegister(&buzzer_config);
    Motor_Init_Config_s driven_conf = {
        // 写一个,剩下的修改方向和id即可
        .can_init_config.can_handle = &hcan1,
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = OPEN_LOOP,
            .close_loop_type = OPEN_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = LK9025,
    };
    driven_conf.can_init_config.tx_id = 1;
    driven[LD] = l_driven = LKMotorInit(&driven_conf);
    driven_conf.can_init_config.tx_id = 2;
    driven[RD] = r_driven = LKMotorInit(&driven_conf);

    Motor_Init_Config_s joint_conf = {
        .can_init_config.can_handle = &hcan2,
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.0f,
                .Ki = 0.0f,
                .Kd = 0.0f,
            },
            .speed_PID = {
                .Kp = 0.0f,
                .Ki = 0.0f,
                .Kd = 0.0f,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = OPEN_LOOP,
            .close_loop_type = ANGLE_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = DMJ4310,
    };
    joint_conf.can_init_config.rx_id = 0;
    joint_conf.can_init_config.tx_id = 1;
    joint[LF] = lf = DMMotorInit(&joint_conf);
    joint_conf.can_init_config.rx_id = 2;
    joint_conf.can_init_config.tx_id = 3;
    joint[LB] = lb = DMMotorInit(&joint_conf);
    joint_conf.can_init_config.rx_id = 4;
    joint_conf.can_init_config.tx_id = 5;
    joint[RF] = rf = DMMotorInit(&joint_conf);
    joint_conf.can_init_config.rx_id = 6;
    joint_conf.can_init_config.tx_id = 7;
    joint[RB] = rb = DMMotorInit(&joint_conf);

    PID_Init_Config_s steer_p_pid_conf = {
        .Kp = 2,
        .Kd = 0.0f,
        .Ki = 0.0f,
        .MaxOut = 4,
        .DeadBand = 0.01f,
        .Improve = PID_DerivativeFilter,
        .Derivative_LPF_RC = 0.05,
    };
    PIDInit(&steer_p_pid, &steer_p_pid_conf);
    PID_Init_Config_s steer_v_pid_conf = {
        .Kp = 2,
        .Kd = 0.0f,
        .Ki = 0.0f,
        .MaxOut = 1,
        .DeadBand = 0.0f,
        .Improve = PID_DerivativeFilter | PID_Integral_Limit,
        .Derivative_LPF_RC = 0.05,
        .IntegralLimit = 2,
    };
    PIDInit(&steer_v_pid, &steer_v_pid_conf);

    PID_Init_Config_s anti_crash_pid_conf = {
        .Kp = 8,
        .Kd = 2.5,
        .Ki = 0,
        .MaxOut = 45,
        .DeadBand = 0.01f,
        .Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit,
        .Derivative_LPF_RC = 0.05,
        .IntegralLimit = 2,
    };
    PIDInit(&anti_crash_pid, &anti_crash_pid_conf);

    PID_Init_Config_s leg_length_pid_conf = {
        .Kp = 1500,
        .Kd = 0,
        .Ki = 120,
        .MaxOut = 200,
        .DeadBand = 0.0001f,
        .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
        .CoefA = 0,
        .CoefB = 0,
        .Derivative_LPF_RC = 0.08,
    };
    PIDInit(&leglen_pid_l, &leg_length_pid_conf);
    PIDInit(&leglen_pid_r, &leg_length_pid_conf);

    PID_Init_Config_s roll_compensate_pid_conf = {
        .Kp = 0.0008f,
        .Kd = 0.00065f,
        .Ki = 0.0f,
        .MaxOut = 0.04,
        .DeadBand = 0.01f,
        .Improve = PID_DerivativeFilter,
        .Derivative_LPF_RC = 0.05,
    };
    PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf);

    l_side.target_len = r_side.target_len = 0.10;
    chassis.vel_cov = 1000; // 初始化速度协方差
    chassis_status = ROBOT_READY;
    DWT_GetDeltaT(&balance_dwt_cnt);
}
static void StartMusic()
{
    static uint16_t buzzercount = 0;
    if (buzzercount < 200)
    {
        buzzercount++;
        AlarmSetStatus(buzzer, ALARM_ON);
    }
    else
    {
        AlarmSetStatus(buzzer, ALARM_OFF);
    }
}
static void EnableAllMotor() /* 打开所有电机 */
{
    for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机
        DMMotorEnable(joint[i]);
    for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机
        LKMotorEnable(driven[i]);
}
static void DisableAllMotor() /* 关闭所有电机 */
{
    for (uint8_t i = 0; i < JOINT_CNT; i++) // 关闭关节电机
        DMMotorStop(joint[i]);
    for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 关闭驱动电机
        LKMotorStop(driven[i]);
}
static void ResetAllMotor() /* 复位关节电机 */
{
    for (uint8_t i = 0; i < DRIVEN_CNT; i++)
        LKMotorSetRef(driven[i], 0);
    for (uint8_t i = 0; i < JOINT_CNT; i++)
        DMMotorSetRef(joint[i], 0);
}
static void StatusSelect()
{
    switch (rc_data->rc.switch_left)
    {
    case 1:
        chassis_status = ROBOT_RESET;
        chassis.target_dist = 0.0f;
        chassis.dist = 0.0f;
        break;
    case 3:
        chassis_status = ROBOT_READY;
        break;
    case 2:
        chassis_status = ROBOT_STOP;

        break;
    default:
        chassis_status = ROBOT_STOP;
        break;
    }
}
static void ResetRobot()
{
    LKMotorSetRef(l_driven, 0);
    LKMotorSetRef(r_driven, 0);
    DMMotorSetRef(joint[LF], -3);
    DMMotorSetRef(joint[LB], 3);
    DMMotorSetRef(joint[RF], 3);
    DMMotorSetRef(joint[RB], -3);
}
static void ControlValueSet()
{
    chassis_cmd_recv.vx = 0.002 * (float)rc_data->rc.rocker_l1;
    // chassis_cmd_recv.offset_angle = 0.001 * (float)rc_data->rc.rocker_r_;
    chassis_cmd_recv.delta_leglen = -0.0000015f * (float)rc_data->rc.rocker_r1;
    chassis_cmd_recv.offset_angle = 0.0000030f * (float)rc_data->rc.rocker_r_;
    l_side.target_len += chassis_cmd_recv.delta_leglen;
    r_side.target_len += chassis_cmd_recv.delta_leglen;
    VAL_LIMIT(l_side.target_len, 0.10, 0.24); // 腿长限幅
    VAL_LIMIT(r_side.target_len, 0.10, 0.24);

    chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;

    // 模型距离参考输入
    chassis.target_dist += chassis.target_v * del_t;
    chassis.target_yaw -= chassis_cmd_recv.offset_angle; // 云台和底盘对齐时电机编码器的单圈反馈角度
}

static void ParamAssemble()
{ // 机体参数,视为平面刚体
    chassis.pitch = (-imu_data->Roll + BALANCE_GRAVITY_BIAS) * DEGREE_2_RAD;
    chassis.pitch_w = -imu_data->Gyro[1];
    chassis.yaw = imu_data->YawTotalAngle * DEGREE_2_RAD;
    chassis.wz = imu_data->Gyro[2];
    chassis.roll = (imu_data->Pitch - ROLL_GRAVITY_BIAS) * DEGREE_2_RAD;
    chassis.roll_w = imu_data->Gyro[0];
    // HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正
    l_side.phi1 = PI + LIMIT_LINK_RAD + lb->measure.position;
    l_side.phi4 = lf->measure.position - LIMIT_LINK_RAD;
    l_side.phi1_w = lb->measure.velocity;
    l_side.phi4_w = lf->measure.velocity;
    l_side.w_ecd = l_driven->measure.speed_rads;
    l_side.back_motor_torque = lb->measure.torque;
    l_side.front_motor_torque = lf->measure.torque;
    r_side.phi1 = PI + LIMIT_LINK_RAD - rb->measure.position;
    r_side.phi4 = -rf->measure.position - LIMIT_LINK_RAD;
    r_side.phi1_w = -rb->measure.velocity;
    r_side.phi4_w = -rf->measure.velocity;
    r_side.w_ecd = -r_driven->measure.speed_rads;
    r_side.back_motor_torque = rb->measure.torque;
    r_side.front_motor_torque = rf->measure.torque;
}
/**
 * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
 * @note 得到的腿部力矩输出还要经过综合运动控制系统补偿后映射为两个关节电机输出
 *
 */
static void CalcLQR(LinkNPodParam *p)
{
    static float k[12][4] = {-783.921412, 549.374923, -144.952445, 1.566761, -44.051541, 28.409620, -6.585319, -0.093785, -1542.076119, 1044.419523, -248.532829, 0.725466, -897.932918, 597.403946, -138.309907, -0.880200, -3601.851288, 2678.780000, -715.534933, 72.886312, -54.874129, 43.008941, -12.513959, 1.463442, -30.944643, 97.955944, -50.267715, 8.171631, -23.066744, 19.893876, -6.122486, 0.716461, -378.029528, 409.447823, -150.372298, 20.573229, -355.697213, 330.243807, -108.788111, 13.671175, 4150.938130, -2707.169824, 597.054761, 50.815891, 73.394231, -48.337821, 10.793149, 0.877980};
    float T[2] = {0}; // 0 T_wheel  1 T_hip
    float l = p->leg_len;
    float lsqr = l * l;
    float lsqr3 = lsqr * l;
    static float lqr_out[12];
    float dist_limit = abs(chassis.target_dist - chassis.dist) > MAX_DIST_TRACK ? sign(chassis.target_dist - chassis.dist) * MAX_DIST_TRACK : (chassis.target_dist - chassis.dist); // todo设置值
    float vel_limit = abs(chassis.target_v - chassis.vel) > MAX_VEL_TRACK ? sign(chassis.target_v - chassis.vel) * MAX_VEL_TRACK : (chassis.target_v - chassis.vel);
    for (uint8_t i = 0; i < 2; ++i)
    {
        uint8_t j = i * 6;
        T[i] = (k[j + 0][0] * lsqr3 + k[j + 0][1] * lsqr + k[j + 0][2] * l + k[j + 0][3]) * -p->theta +
               (k[j + 1][0] * lsqr3 + k[j + 1][1] * lsqr + k[j + 1][2] * l + k[j + 1][3]) * -p->theta_w +
               (k[j + 2][0] * lsqr3 + k[j + 2][1] * lsqr + k[j + 2][2] * l + k[j + 2][3]) * (chassis.target_dist - chassis.dist) +
               (k[j + 3][0] * lsqr3 + k[j + 3][1] * lsqr + k[j + 3][2] * l + k[j + 3][3]) * (chassis.target_v - chassis.vel) +
               (k[j + 4][0] * lsqr3 + k[j + 4][1] * lsqr + k[j + 4][2] * l + k[j + 4][3]) * -chassis.pitch +
               (k[j + 5][0] * lsqr3 + k[j + 5][1] * lsqr + k[j + 5][2] * l + k[j + 5][3]) * -chassis.pitch_w;
    }
    p->T_wheel = T[0];
    p->T_hip = T[1];
}

static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
{
    float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
    PIDCalculate(&steer_v_pid, chassis.wz, p_ref); // 双环

    l_side.T_wheel -= steer_v_pid.Output;
    r_side.T_wheel += steer_v_pid.Output;

    volatile static float swerving_speed_ff, ff_coef = 1;
    swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈
    PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0);
    l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff;
    r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
   
}

static void LegControl() /* 腿长控制和Roll补偿 */
{
    PIDCalculate(&roll_compensate_pid, chassis.roll, 0);
    l_side.target_len += roll_compensate_pid.Output;
    r_side.target_len -= roll_compensate_pid.Output;
    VAL_LIMIT(l_side.target_len, 0.10, 0.24); // 腿长限幅
    VAL_LIMIT(r_side.target_len, 0.10, 0.24);
    static float gravity_comp = 0;
    static float roll_extra_comp_p = 0;
    float roll_comp = roll_extra_comp_p * chassis.roll;
    leglen_pid_l.Kp = leglenKp;
    leglen_pid_l.Ki = leglenKi;
    leglen_pid_l.Kd = leglenKd;
    leglen_pid_r.Kp = leglenKp;
    leglen_pid_r.Ki = leglenKi;
    leglen_pid_r.Kd = leglenKd;
    // l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp + roll_comp;
    // r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp + 2 - roll_comp;
    l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len);
    r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len);
    // @todo: 还需要加和roll的纯Kp项
}

static void WattLimitSet() /* 设定运动模态的输出 */
{
    DMMotorSetRef(lf, -l_side.T_front); // 根据扭矩常数计算得到的系数
    DMMotorSetRef(lb, -l_side.T_back);
    DMMotorSetRef(rf, r_side.T_front);
    DMMotorSetRef(rb, r_side.T_back);
    LKMotorSetRef(l_driven, 252.52 * l_side.T_wheel);
    LKMotorSetRef(r_driven, 252.52 * -r_side.T_wheel);
}
uint8_t reset_flag = 0;
void BalanceTask()
{
    del_t = DWT_GetDeltaT(&balance_dwt_cnt);
    StartMusic();
    StatusSelect(); // 模式选择

    ControlValueSet(); // 控制量设置

    ParamAssemble();
    // 将五连杆映射成单杆
    Link2Leg(&l_side, &chassis);
    Link2Leg(&r_side, &chassis);
    // 通过卡尔曼滤波估计机体速度
    SpeedEstimation(&l_side, &r_side, &chassis, imu_data, del_t);

    // 根据单杆计算处的角度和杆长,计算反馈增益
    CalcLQR(&l_side);
    CalcLQR(&r_side);
    // 转向和抗劈叉
    SynthesizeMotion();
    // 腿长控制,保持机体水平
    LegControl();
    // VMC映射成关节输出
    VMCProject(&l_side);
    VMCProject(&r_side);
NormalforceSolve(&l_side, &r_side,imu_data, del_t);
    // if (abs(l_side.T_wheel) < 0.1f && abs(r_side.T_wheel) < 0.1f)
    // {
    //     AlarmSetStatus(buzzer, ALARM_ON);
    // }
    // else
    // {
    //     AlarmSetStatus(buzzer, ALARM_OFF);
    // }
    switch (chassis_status)
    {
    case ROBOT_READY:
        EnableAllMotor();
        WattLimitSet();
        break;
    case ROBOT_STOP:
        DisableAllMotor();
        reset_flag = 1;
        break;
    case ROBOT_RESET:
        DisableAllMotor();
        break;
    default:
        DisableAllMotor();
        break;
    }
}